import time
from pymavlink import mavutil

import threading

connection_string = '/dev/ttyS1' # заменить на соответствующий порт
baud_rate = 57600 # скорость передачи данных

data = []

def gen_payload(op, filename=None, file=None):
    if file == None:
        file = []
    if op == "get":
        payload = [0] * 251
        data = list(filename.encode())
        start_index=12
        end_index = start_index + len(data)
#        print(data)
        payload[start_index:end_index]=data
#        print(payload)
        payload[0] = 1
        payload[2] = 3
        payload[3] = 4
        payload[4] = len(data)
        payload2 = [0] * 251
        payload2[0] = payload[0]+1
        payload2[2] = 3
        payload2[3] = 15
        payload2[4] = 80
        return(payload,payload2)
    if op == "put" and file != None:
        file_payload = []
        block_len = 80
        file_len = len(file)
        if file_len >= block_len:
            blocks = file_len//block_len+1
        else:
            blocks = 1

        payload = [0] * 251
        data = list(filename.encode())
        start_index=12
        end_index = start_index + len(data)
        payload[start_index:end_index]=data
        payload[0] = 1
        payload[2] = 3
        payload[3] = 6
        payload[4] = len(data)
        file_payload.append(payload)
        for block in range(blocks):
            payload2 = [0] * 251
            payload2[0] = 2+block
            payload2[2] = 3
            payload2[3] = 7
            start_index=12
            offset = block*block_len
            payload2[8] = offset & 0xFF  # Младший байт
            payload2[9] = (offset >> 8) & 0xFF
            if block != blocks-1:
                end_index = start_index + block_len
                payload2[4] = block_len
                payload2[start_index:end_index] = file[(block)*block_len:(block+1)*block_len]
            if block == blocks-1:
                end_index = start_index + file_len%block_len
                payload2[4] = file_len%block_len
                print("end index is " + str(end_index))
                payload2[start_index:end_index] = file[(block)*block_len:file_len]
            file_payload.append(payload2)
        file_payload = tuple(file_payload)
        return(file_payload)


def decode_list(nums):
    # Convert list of numbers to a string
    decoded_str = ''.join(chr(num) for num in nums)

    # Split by null character
    entries = decoded_str.split('\x00')

    # Initialize empty lists to store files and directories
    files = {}
    directories = []

    for entry in entries:
        if '\t' in entry:
            # Split entry by tab character to get filename and size
            filename, size = entry.split('\t', 1)
            files[filename] = size
        elif entry:  # Not empty
            directories.append(entry)

    return {
        "files": files,
        "directories": directories
    }

def extract_data(data):
    clear_data = []
    for sublist in data:
        for i in range(12, (len(sublist)-1)):
            if sublist[i]==0 and sublist[i+1]==0:
                break
            clear_data.append(sublist[i])

    return clear_data


def write_file(clear_data):
    with open('out.bin', 'wb') as file:
        # Записываем байты в файл
        file.write(bytes(clear_data))

        
def process_messages(mav):
    global data
    while True:
        try:
            # чтение пакета
            msg = mav.recv_match()
            if not msg:
                continue

            # обработка пакета
            if msg.get_type() == 'FILE_TRANSFER_PROTOCOL':
                print('FTP received')
                print(msg.payload)
                print(msg.payload[6])
                if msg.payload[5] == 15 and msg.payload[3] != 129:
                    data.append(msg.payload)
                if msg.payload[3] == 129:
                    write_file(extract_data(data))
                    
                #resp = decode_list(msg.payload)
                #print(resp)

        except KeyboardInterrupt:
            print('Exiting')
            break
        time.sleep(0.1)

def send_messages(part, mav):
    for part in parts:
        mav.mav.file_transfer_protocol_send(0, 1, 1, part)
        print("Sent")
        

# вывод сообщения в главном потоке


mav = mavutil.mavlink_connection(connection_string, baud=baud_rate)
mav.robust_parsing = True
mav.wait_heartbeat()
print("Started")
file_path = "file.txt"
file_bytes = []

with open(file_path, "rb") as file:
    byte = file.read(1)
    while byte:
        file_bytes.append(ord(byte))
        byte = file.read(1)
parts = gen_payload("get", "output.sig")
print(parts)
# запуск потока для обработки сообщений
receive_thread = threading.Thread(target=process_messages, args=(mav,))
send_thread = threading.Thread(target=send_messages, args=(parts, mav,))
receive_thread.start()
send_thread.start()
